#pragma once
#include "ros/ros.h"
#include "auto_msgs/localizition.h"
#include "auto_msgs/chassis.h"
#include "auto_msgs/control_cmd.h"
#include "auto_msgs/planning.h"
#include "auto_msgs/planning_point.h"
#include "common_tool/clampPid.h"
#include "common_tool/watchDog.h"
#include <osqp_control/debug_lateral.h>
#include "vehicle_lateral_mpc.h"
#include <Eigen/Dense>
#include <Eigen/Sparse>
#include <memory>

namespace auto_ros
{
namespace control
{
class NodeLateral
{
	//    watchDog(int foodPerTurn=0, int hungerPerTurn=0, int hungerMin=0,\
    //int criticalPoint=50, int hungerMax=100);
private:
	watchDog localizition_dog_;
	watchDog chassis_dog_;
	watchDog planning_dog_;
	auto_msgs::planning planning_msg_;
	auto_msgs::localizition localizition_msg_;
	auto_msgs::chassis chassis_msg_;
	osqp_control::debug_lateral debug_msg_;
	void UpdateDebugMsg();
	void NoFood();

	//ref point related
	auto_msgs::planning_point ref_point_closet_;
	auto_msgs::planning_point ref_point_second_closet_;
	auto_msgs::planning_point ref_point_interp_;
	double ref_lambda_;
	/////////////
	std::shared_ptr<VehicleLateralMpc> mpc_ptr_;
	YAML::Node node_yaml_;
	std::string log_file_name_;
	Eigen::VectorXd x0_;
	Eigen::VectorXd u_fb_;
	Eigen::MatrixXd distur_ref_;
	double speed_;
	double speed_cmd_ = 0;
	double last_steer_angle_ = 0;
	//x0

public:
	clampPid speed_controller_;
	NodeLateral() = delete;
	NodeLateral(double loop_rate);
	void CBLocalizition(const auto_msgs::localizition &msg);
	void CBChassis(const auto_msgs::chassis &msg);
	void CBPlanning(const auto_msgs::planning &msg);
	bool SearchRefPoint(const auto_msgs::planning msg);
	bool CheckDog();
	void LoopProc();
	void HardBrake();
	bool LateralCtr();
	bool LongtitudeCtr();
	bool UpdateInput();

	//relate with  ros
	// publish apollo wrapper topic
	ros::NodeHandle nh;
	ros::Publisher control_cmd_pub_ = nh.advertise<auto_msgs::control_cmd>("control_cmd", 3);
	auto_msgs::control_cmd control_cmd_;
	ros::Publisher debug_pub_ = nh.advertise<osqp_control::debug_lateral>("debug_lateral", 3);

	//pub debug msg
	//ros::Publisher pure_pursuit_debug_pub =nh.advertise
	//    <pure_pursuit_pkg::pure_pursuit_debug>("pure_pursuit_debug", 3);
	//subscribe apollo wrapper topic
	ros::Rate loop_rate_;
	double rate_;
	ros::Subscriber localizition_sub_ =
		nh.subscribe("localizition", 3, &NodeLateral::CBLocalizition, this);
	ros::Subscriber chassis_sub_ =
		nh.subscribe("chassis", 3, &NodeLateral::CBChassis, this);
	ros::Subscriber planning_sub_ =
		nh.subscribe("planning_path", 3, &NodeLateral::CBPlanning, this);
};
} // namespace control
} // namespace auto_ros